function [collision, collision_direction] = check_collision(pose, obstacles, safety_margin)
% 检测机器人是否与障碍物碰撞或即将碰撞
% 输入: pose-机器人位姿[x;y;theta], obstacles-障碍物列表, safety_margin-安全距离
% 输出: collision-是否碰撞, collision_direction-碰撞方向

    x = pose(1);
    y = pose(2);
    
    collision = false;
    collision_direction = [0; 0];  % [dx, dy] 指向远离障碍物的方向
    
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        x_min = obs(1);
        y_min = obs(2);
        x_max = obs(3);
        y_max = obs(4);
        
        % 计算机器人到矩形的最近距离
        closest_x = max(x_min, min(x, x_max));
        closest_y = max(y_min, min(y, y_max));
        
        dx = x - closest_x;
        dy = y - closest_y;
        dist = sqrt(dx^2 + dy^2);
        
        % 检查是否在安全距离内
        if dist < safety_margin
            collision = true;
            
            % 计算逃离方向
            if dist > 0.01
                collision_direction(1) = collision_direction(1) + dx / dist;
                collision_direction(2) = collision_direction(2) + dy / dist;
            else
                % 在障碍物内，选择最近的边逃离
                dist_to_left = abs(x - x_min);
                dist_to_right = abs(x - x_max);
                dist_to_bottom = abs(y - y_min);
                dist_to_top = abs(y - y_max);
                
                min_dist = min([dist_to_left, dist_to_right, dist_to_bottom, dist_to_top]);
                
                if min_dist == dist_to_left
                    collision_direction(1) = collision_direction(1) - 1;
                elseif min_dist == dist_to_right
                    collision_direction(1) = collision_direction(1) + 1;
                elseif min_dist == dist_to_bottom
                    collision_direction(2) = collision_direction(2) - 1;
                else
                    collision_direction(2) = collision_direction(2) + 1;
                end
            end
        end
    end
    
    % 归一化逃离方向
    if norm(collision_direction) > 0
        collision_direction = collision_direction / norm(collision_direction);
    end
end

